#include <Servo.h>

/****************************/
/* servo parts              */
/****************************/
#define N_HEAD 2
#define I_HEAD_NECK1 0
#define I_HEAD_NECK2 1

#define N_ARM 4
#define I_ARM_SHOULDER 0
#define I_ARM_BICEP 1
#define I_ARM_ELBOW 2
#define I_ARM_WRIST 3

#define N_LEG 5
#define I_LEG_HIP1 0
#define I_LEG_HIP2 1
#define I_LEG_THIGH 2
#define I_LEG_KNEE 3
#define I_LEG_ANKLE 4

Servo servo_head[N_HEAD];
int ns_head[N_HEAD] = {
  26, 27};
int pos_head[N_HEAD] = {
  90, 90};

Servo servo_left_arm[N_ARM];
int ns_left_arm[N_ARM] = {
  46, 47, 48, 49};
int pos_left_arm[N_ARM] = {
  90-60, 180, 180, 90};

Servo servo_right_arm[N_ARM];
int ns_right_arm[N_ARM] = {
  33, 34, 35, 36};
int pos_right_arm[N_ARM] = {
  90+60, 0, 0, 90};

Servo servo_left_leg[N_LEG];
int ns_left_leg[N_LEG] = {
  41, 42, 43, 44, 45};
int pos_left_leg[N_LEG] = {
  90, 90, 90, 90, 90};

Servo servo_right_leg[N_LEG];
int ns_right_leg[N_LEG] = {
  28, 29, 30, 31, 32};
int pos_right_leg[N_LEG] = {
  90, 90, 90, 90, 90};

/****************************/
/* positions                */
/****************************/
// current position !
#define _POSITION_CURRENT 0

// static stands
#define _POSITION_RIGHT 0x0100
#define _POSITION_STAND 0x0101
#define _POSITION_WAITFOR 0x0102

// hello stands
#define _POSITION_HELLO1 0x0200
#define _POSITION_HELLO2 0x0201

// sporty
#define _POSITION_PUMP0 0x0300
#define _POSITION_PUMP1 0x0301
#define _POSITION_SEMIPUMP0 0x0310
#define _POSITION_SEMIPUMP1 0x0311

// exotic :)
#define _POSITION_HULK 0x0910
#define _POSITION_PLANE 0x0920

class AnimLoopStep
{
public:
  unsigned long _iTime;  
  int _iPos1, _iPos2;
  AnimLoopStep()
  {
    _iTime = 0;
    _iPos1 = 0;
    _iPos2 = 0;
  }
  AnimLoopStep(unsigned long iTime, int iPos1, int iPos2)
  {
    _iTime = iTime;
    _iPos1 = iPos1;
    _iPos2 = iPos2;    
  }
};

class AnimLoop
{
public:
  char name[16];
  int _iSteps;
  AnimLoopStep _steps[10];
  AnimLoop()
  {
    strcpy(name, "anim");
    _iSteps = 0;
  }
};

#define N_ANIM_LOOPS 5
AnimLoop loops[N_ANIM_LOOPS];

unsigned long g_time=0;
void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);

  init_servos();
  init_loops();

  Serial.println("You are in manual mode, press 'a' to switch to loops mode.");

  g_time = millis();

  set_animation_step(500, _POSITION_RIGHT, _POSITION_RIGHT, 0);
}

boolean g_anim = false;
int g_what_anim = 0;
void loop()
{
  // first update servo position
  update_servos();

  // set next position
  unsigned long cur_time = millis();
  unsigned long slicetime = cur_time-g_time;
  g_time = cur_time;

  // Is in animation loop ?
  if(is_in_animation())
  {
    // animation lop is over ?
    if(!do_animation(slicetime)) return;
  }

  // got bluetooth command ?
  if(check_commands()) return;
  
  delay(100);
}

